#include <ros/ros.h>
#include <signal.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <string>
#include <vector>
#include <map>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Pose2D.h>
#include<sys/socket.h>
#include<netinet/in.h>
#include<sys/types.h>
#include<sys/socket.h>
#include<netinet/in.h>
#include<arpa/inet.h>
#include <ecl/threads.hpp>

using namespace std;
using Client =  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ;

ros::Publisher *cmdVelPubPtr;
Client *actionClient;
geometry_msgs::Point current_point;
geometry_msgs::Pose2D base_p_;
ecl::Thread thread_;

void shutdown(int sig);


void shutdown(int sig)
{
     actionClient->cancelAllGoals();
   	ros::Duration(2).sleep(); 

	cmdVelPubPtr->publish(geometry_msgs::Twist());
	ros::Duration(1).sleep(); 
	ROS_INFO("Stopped the robot!");
	ros::shutdown();
}

void thread_func()
{
	ros::spin();
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "scene_deploy");
	ros::NodeHandle node;
	ros::Rate loop_rate(10);
	signal(SIGINT, shutdown);
	ROS_INFO("Scene Deploy Node Start...");
	ros::Publisher cmdVelPub = node.advertise<geometry_msgs::Twist>("/raw_cmd_vel", 5);
	cmdVelPubPtr = &cmdVelPub;

	//Cycle through the four waypoints
	string taskParamName = "/usr_task";
	int task=0;
    int cont = 0;
	geometry_msgs::Twist pub_msg_vel;
	while (1)
	{
		/* code */
		ROS_INFO("Waiting for usr_task 2 or 3...");
		do {
			ros::param::get(taskParamName,task);
			if(task) break;
			ros::Duration(0.2).sleep(); 
		}while ( 1 );
        while(1){
            if(task == 2) { pub_msg_vel.linear.x = -0.15;}
            else if(task == 3) { pub_msg_vel.linear.x = 0.15;}
		    cmdVelPubPtr->publish(pub_msg_vel);
		    ros::Duration(0.1).sleep(); 
            cont++;
            if(cont == 100){cont = 0 ;break;}
        }
        
        pub_msg_vel.linear.x = 0.0;
		cmdVelPubPtr->publish(pub_msg_vel);
		ros::param::set("/usr_task",0);

	}

	ROS_INFO("End...");
	return 0;
}
